-module( kinematics_object ).

-export( [start/6] ).

-record( object, { world, worker, ctrlState, ctrlUpdate, ctrlMessage, cellPid, cellBounds, pos, vel, lastUpdate } ).

-define( MSG_TIMEOUT, ( 10 * 1000 ) ).



start( World, P, V, CtrlState, CtrlUpdateFun, CtrlMsgFun ) when
is_pid(World), is_tuple(P), size(P)=:=3, is_tuple(V), size(V)=:=3, is_function(CtrlUpdateFun, 4), is_function(CtrlMsgFun, 4) ->
  State = #object{ world=World, ctrlState=CtrlState, ctrlUpdate=CtrlUpdateFun, ctrlMessage=CtrlMsgFun, pos=P, vel=V, lastUpdate = now() },
  assignWorker( State ).

  

assignWorker( #object{ world=World } = State ) ->
  World ! { assign_worker, self() },
  receive
    { worker_assigned, Worker } ->
      changeCell( State#object{ worker=Worker } )
  end.



objectLoop( State ) ->
  receive
    { set_object, NewP, NewV } ->
      updateObject( State#object{ pos = NewP, vel = NewV } );

    { set_position, NewPos } ->
      updateObject( State#object{ pos = NewPos } );

    { set_velocity, NewVel } ->
      updateObject( State#object{ vel = NewVel } );

    { control_msg, Msg } ->
      controlObject( State, Msg );

    dump ->
      error_logger:info_msg( "Current position: ~p\n", [State#object.pos] ),
      updateObject( State );

    { dbg_vis, Vis } ->
      dbgVisObject( State, Vis ),
      updateObject( State )

    after 50 ->
      updateObject( State )
  end.

  
  
checkDeltaTime( DT ) when ( DT < 0.1 ) -> ok;
checkDeltaTime( DT ) -> error_logger:warning_msg( "Object ~p step exceeded: ~p\n", [self(), DT] ).
  
  
updateObject( State = #object{ ctrlUpdate=CtrlUpdF, ctrlState=CtrlSt, pos=P, vel=V } ) ->
  NewTime = now(),
  DT = timer:now_diff( NewTime, State#object.lastUpdate ) * 1.0e-6,
  checkDeltaTime( DT ),
  MovedPos = geom:vec3ma( P, V, DT ),
  case CtrlUpdF( CtrlSt, MovedPos, V, DT ) of
    { stop, Reason } ->
      error_logger:info_msg( "Stopping object ~p for ~p\n", [self(), Reason] );
    { continue, UpdCtlState, UpdPos, UpdVel } ->
      checkCell( State#object{ ctrlState=UpdCtlState, pos=UpdPos, vel=UpdVel, lastUpdate = NewTime } )
  end.

  
controlObject( State = #object{ ctrlMessage=Ctrl, ctrlState=CtrlSt, pos=P, vel=V }, Msg ) ->
  %error_logger:info_msg( "Control msg: ~p\n", [Msg] ),
  case Ctrl( CtrlSt, Msg, P, V ) of
    { stop, Reason } ->
      error_logger:info_msg( "Stopping object ~p for ~p\n", [self(), Reason] );
    { continue, UpdCtlState, UpdPos, UpdVel } ->
      updateObject( State#object{ ctrlState=UpdCtlState, pos=UpdPos, vel=UpdVel } )
  end.

  
  
checkCell( State ) ->
  case geom:boxIn( State#object.cellBounds, State#object.pos ) of
    true -> objectLoop( State );
    false -> changeCell( State )
  end.



changeCell( State ) ->
  State#object.worker ! { assign_cell, self(), State#object.pos },
  receive
    { cell_assigned, { Bounds, CellPid } } -> transferToACell( State, Bounds, CellPid )
    after ?MSG_TIMEOUT -> exit( message_wait_timeout )
  end.
  



transferToACell( State, _CellBounds, CellPid ) %Object stayed in a same cell; Typecally it means that it is out of the world's boundaries
when ( State#object.cellPid =:= CellPid ) ->
  objectLoop( State );

  
transferToACell( State, CellBounds, CellPid ) ->
  error_logger:info_msg( "Object ~p at ~p assigned a cell ~p\n", [self(), State#object.pos, CellPid] ),

  case ( State#object.cellPid ) of
    undefined -> true;
    OldCellPid ->
      OldCellPid ! { remove_object, self() },
      receive
        { object_removed, OldCellPid } -> true
        after ?MSG_TIMEOUT -> exit( message_wait_timeout )
      end
  end,

  CellPid ! { add_object, self() },
  receive
    { object_added, CellPid } ->
      objectLoop( State#object{ cellPid = CellPid, cellBounds = CellBounds } )
    after ?MSG_TIMEOUT ->
      exit( message_wait_timeout )
  end.



dbgVisObject( State, Vis ) ->
  { X, Y, _ } = State#object.pos,
  { VX, VY, _ } = State#object.vel,
  vis:remove( Vis, kin_obj, self() ),
  vis:addDiamond( Vis, X, Y, 4.0, [ {tag1, kin_obj}, {tag2, self()}, {color, blue} ] ),
  vis:addLine( Vis, X, Y, X + VX, Y + VY, [ {tag1, kin_obj}, {tag2, self()}, {color, green} ] ).
